#include "motor.h"

extern UART_HandleTypeDef huart2;



// 继电器吧CRC校验和计算
unsigned short crc16(const unsigned char *data, int length)
{
    unsigned short crc = 0xffff;
    for (int i = 0; i < length; ++i)
    {
        crc ^= data[i];
        for (int j = 7; j >= 0; --j)
        {
            if ((crc & 0x0001) != 0)
            {
                crc >>= 1;
                crc ^= 0xa001;
            } else
            {
                crc >>= 1;
            }
        }
    }
    return crc;
}

// 继电器板控制
void relaycontrol(uint8_t cmd)
{
    // 定义4个指令数组，索引0-3分别对应K10、K11、K12、K16
    uint8_t commands[4][8] = {
        {0x01, 0x06, 0x00, 0x0c, 0x00, 0x00, 0, 0},  // K10 (索引0)
        {0x01, 0x06, 0x00, 0x0d, 0x00, 0x00, 0, 0},  // K11 (索引1)
        {0x01, 0x06, 0x00, 0x0e, 0x00, 0x00, 0, 0},  // K12 (索引2)
        {0x01, 0x06, 0x00, 0x0f, 0x00, 0x00, 0, 0}   // K16 (索引3)
    };

    // 定义停止组和启动组的索引（动态变化）
    int stop_group[2];   // 先发送的停止组（值为0）
    int start_group[2];  // 后发送的启动组（值为1）

    if (cmd == 1) {
        // 正转逻辑：先停K12、K10，再启K16、K11
        stop_group[0] = 2;  // K12的索引
        stop_group[1] = 0;  // K10的索引
        start_group[0] = 3; // K16的索引
        start_group[1] = 1; // K11的索引
    } else {
        // 反转逻辑：先停K16、K11，再启K12、K10
        stop_group[0] = 3;  // K16的索引
        stop_group[1] = 1;  // K11的索引
        start_group[0] = 2; // K12的索引
        start_group[1] = 0; // K10的索引
    }

    // 第一步：处理停止组（设置值为0并发送）
    for (int i = 0; i < 2; i++)
    {
        int idx = stop_group[i];
        commands[idx][5] = 0x00;  // 停止值
        unsigned short crc = crc16(commands[idx], 6);
        commands[idx][6] = crc & 0x00FF;
        commands[idx][7] = crc >> 8;
        HAL_UART_Transmit(&huart2, commands[idx], 8, 0xff);
    }

    // 延时
    HAL_Delay(100);
    printf(cmd == 1 ? "正转\r\n" : "反转\r\n");

    // 第二步：处理启动组（设置值为1并发送）
    for (int i = 0; i < 2; i++)
    {
        int idx = start_group[i];
        commands[idx][5] = 0x01;  // 启动值
        unsigned short crc = crc16(commands[idx], 6);
        commands[idx][6] = crc & 0x00FF;
        commands[idx][7] = crc >> 8;
        HAL_UART_Transmit(&huart2, commands[idx], 8, 0xff);
    }
}
